#include "bsp_pid.h"
#include "bsp.h"


#define PI      (3.1415926f)

motor_pid_t pid_motor[4];



// Example Initialize PID parameters 初始化PID参数
void PID_Param_Init(void)
{
    for (int i = 0; i < MAX_MOTOR; i++)
    {
        pid_motor[i].target_val = 0.0;
        pid_motor[i].pwm_output = 0.0;
        pid_motor[i].err = 0.0;
        pid_motor[i].err_last = 0.0;
        pid_motor[i].err_next = 0.0;
        pid_motor[i].integral = 0.0;

        pid_motor[i].Kp = PID_DEF_KP;
        pid_motor[i].Ki = PID_DEF_KI;
        pid_motor[i].Kd = PID_DEF_KD;
    }
}


//void adjustAndSyncMotors()
// {
//    int baseStep = motors[0].currentStep; // 选取第一个电机作为基准
//
//    for(int i = 1; i < MOTOR_COUNT; i++)
//     {
//        int stepDiff = baseStep - motors[i].currentStep;
//        if(stepDiff != 0)
//        {
//            // 根据stepDiff调整电机速度，这里简化处理，实际情况可能更复杂
//            adjust_motor_speed(i, stepDiff);
//        }
//    }
//}


/**
 * 计算PID控制器的输出增量-增量式PID计算公式。
 * 根据当前误差、前一误差和上一误差计算控制增量，以逐步接近目标值。
 * 
 * @param pid 指向PID控制器结构体的指针，包含控制器的参数和状态。
 * @param actual_val 当前实际值，用于计算误差。
 * @return 计算得到的PWM输出增量。
 */
// Incremental PID calculation formula  增量式PID计算公式
float PID_Incre_Calc(motor_pid_t *pid, float actual_val)    //PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]);
{
    // 计算当前误差：目标值与实际值之差
    pid->err = pid->target_val - actual_val;

//    g_speed_L1 = pid->target_val;     //测试用		//串口打印，:-200, 2033, -2233, 0
//    g_speed_L2 = actual_val;          //测试用
//    g_speed_R1 = pid->err;            //测试用
//    g_speed_R2 = 0;                   //测试用
    
    // 根据比例、积分和微分计算PWM输出的增量
    pid->pwm_output += pid->Kp * (pid->err - pid->err_next)                         //(0.8f)
                      + pid->Ki * pid->err                                          //(0.06f)
                      + pid->Kd * (pid->err - 2 * pid->err_next + pid->err_last);   //(0.5f)

//    g_speed_L1 = (pid->err - pid->err_next);						//测试用0
//    g_speed_L2 = (pid->err - 2 * pid->err_next + pid->err_last);	//测试用0
//    g_speed_R1 = pid->Kp * (pid->err - pid->err_next) + pid->Ki * pid->err + pid->Kd * (pid->err - 2 * pid->err_next + pid->err_last);	//测试用-143
//    g_speed_R2 = pid->pwm_output;									//测试用-3743

//    g_speed_L1 = pid->err_last;		//测试用-3003,	-2948,
//    g_speed_L2 = pid->err_next;		//测试用-2948,	-2948,
//    g_speed_R1 = pid->err;			//测试用-2948,	-2948,
//    g_speed_R2 = pid->pwm_output;		//测试用-3804,	-3776,

    // 更新误差历史：当前误差成为前一误差，前一误差成为上一误差
    pid->err_last = pid->err_next;
    pid->err_next = pid->err;
    
//    g_speed_L1 = pid->err_last;		//测试用-2948,	-3003,
//    g_speed_L2 = pid->err_next;		//测试用-3003,	-3058,
//    g_speed_R1 = pid->err;			//测试用-3003,	-3058,
//    g_speed_R2 = pid->pwm_output;		//测试用-3824,	-3854,

    // 限制PWM输出值在允许的范围内
    if (pid->pwm_output > MOTOR_MAX_PULSE)  
        pid->pwm_output = MOTOR_MAX_PULSE;
    if (pid->pwm_output < -MOTOR_MAX_PULSE) 
        pid->pwm_output = -MOTOR_MAX_PULSE;
        
    // 返回计算得到的PWM输出增量
//    g_speed_R2 = pid->pwm_output;		//测试用-3600,
    return pid->pwm_output;
}


/**
 * @brief 计算电机的PWM值通过PID控制算法
 * 
 * 本函数通过PID控制算法计算每个电机对应的速度PWM值。
 * 它遍历所有电机，对每个电机的速度进行PID计算，将计算结果赋值给相应的PWM值。
 * 
 * @param motor 电机数据结构体指针，包含电机速度和PWM值等信息。
 */
// PID Calculates the output value  PID计算输出值
void PID_Calc_Motor(motor_data_t* motor)
{
    //遍历所有电机，MAX_MOTOR定义了电机的数量
    for (int i = 0; i < MAX_MOTOR; i++)
    {
        //调用PID增量计算函数，计算当前电机的速度PWM值
        //pid_motor[i]是PID控制器的实例，speed_mm_s[i]是当前电机的速度
        motor->speed_pwm[i] = PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]);

//		g_speed_L1 = motor->speed_mm_s[0];	//测试用  串口输出值为494, 1044, 989, 989，M1速度明显低
//		g_speed_L2 = motor->speed_mm_s[1];	//测试用
//		g_speed_R1 = motor->speed_mm_s[2];	//测试用
//		g_speed_R2 = motor->speed_mm_s[3];	//测试用

//		g_speed_L1 = motor->speed_pwm[0];	//测试用  串口输出值为-3600, -3575, -3596, -3599，但还是M1的实际霍尔值会少
//		g_speed_L2 = motor->speed_pwm[1];	//测试用
//		g_speed_R1 = motor->speed_pwm[2];	//测试用
//		g_speed_R2 = motor->speed_pwm[3];	//测试用
    }
}


/**
 * 设置电机的PID参数。
 * 
 * 本函数用于为指定的电机ID设置比例(P)、积分(I)和微分(D)控制器的参数。
 * 如果指定的电机ID等于MAX_MOTOR，那么将为所有电机设置相同的PID参数。
 * 这种设计允许在一次调用中快速配置所有电机的PID参数，提高了配置过程的效率。
 * 
 * @param motor_id 电机ID，用于识别要配置的电机。
 * @param kp 比例控制器的增益参数。
 * @param ki 积分控制器的增益参数。
 * @param kd 微分控制器的增益参数。
 */
// 设置PID参数，motor_id=4设置所有，=0123设置对应电机的PID参数。
// Set PID parameters, motor_id=4 set all, =0123 Set PID parameters of the corresponding motor
void PID_Set_Motor_Parm(uint8_t motor_id, float kp, float ki, float kd)
{
    // 如果指定的电机ID超出定义的最大电机数量，则直接返回，不进行任何设置。
    if (motor_id > MAX_MOTOR) return;

    // 如果指定的电机ID等于MAX_MOTOR，那么为所有电机设置相同的PID参数。
    if (motor_id == MAX_MOTOR)
    {
        for (int i = 0; i < MAX_MOTOR; i++)
        {
            pid_motor[i].Kp = kp;
            pid_motor[i].Ki = ki;
            pid_motor[i].Kd = kd;
        }
    }
    // 如果指定的电机ID是一个特定的电机，那么只为此电机设置PID参数。
    else
    {
        pid_motor[motor_id].Kp = kp;
        pid_motor[motor_id].Ki = ki;
        pid_motor[motor_id].Kd = kd;
    }
}

// Clearing PID Data  清除PID数据
/**
 * 清除指定电机或所有电机的PID控制参数。
 * 
 * @param motor_id 电机编号。如果该参数等于MAX_MOTOR，则清除所有电机的PID参数；
 *                 否则，只清除指定电机的PID参数。
 * 
 * 此函数用于将指定电机或所有电机的PWM输出、误差及其历史值、积分项重置为0，
 * 以便在新的控制周期开始时，PID控制器能够根据新的目标值进行计算。
 * 这对于确保PID控制器的正确重启和避免之前的控制误差对当前控制过程的影响至关重要。
 */
void PID_Clear_Motor(uint8_t motor_id)
{
    // 如果指定的电机编号超过最大电机数量，直接返回，不进行任何操作
    if (motor_id > MAX_MOTOR) return;

    // 如果指定的电机编号为最大电机数量，说明需要清除所有电机的PID参数
    if (motor_id == MAX_MOTOR)
    {
        // 遍历所有电机，将它们的PID参数重置为0
        for (int i = 0; i < MAX_MOTOR; i++)
        {
            pid_motor[i].pwm_output = 0.0;  // PWM输出重置为0
            pid_motor[i].err = 0.0;         // 当前误差重置为0
            pid_motor[i].err_last = 0.0;    // 上一周期误差重置为0
            pid_motor[i].err_next = 0.0;    // 下一周期预测误差重置为0
            pid_motor[i].integral = 0.0;    // 积分项重置为0
        }
    }
    else
    {
        // 如果指定的电机编号不是最大电机数量，只清除指定电机的PID参数
        pid_motor[motor_id].pwm_output = 0.0;   // PWM输出重置为0
        pid_motor[motor_id].err = 0.0;          // 当前误差重置为0
        pid_motor[motor_id].err_last = 0.0;     // 上一周期误差重置为0
        pid_motor[motor_id].err_next = 0.0;     // 下一周期预测误差重置为0
        pid_motor[motor_id].integral = 0.0;     // 积分项重置为0
    }
}

// Set PID target speed, unit: mm/s  设置PID目标速度，单位为：mm/s
/**
 * 设置电机的PID目标值
 * 
 * 本函数用于设定指定电机的PID控制器的目标值。通过PID算法，电机将试图达到这个目标值。
 * 
 * @param motor_id 电机标识符，用于区分不同的电机。电机ID不能超过定义的最大电机数量。
 * @param target 目标值，即希望电机达到的速度或位置等。
 * 
 * 注意：如果传入的motor_id等于最大电机数量，那么函数将为所有电机设置相同的目标值。
 * 这种设计可能用于某些情况下需要所有电机同时达到相同目标值的场景。
 */
void PID_Set_Motor_Target(uint8_t motor_id, float target)
{
    // 如果电机ID大于预设的最大电机数量，则直接返回，不进行后续操作。
    if (motor_id > MAX_MOTOR) return;

    // 如果电机ID等于最大电机数量，那么为所有电机设置相同的目标值。
    if (motor_id == MAX_MOTOR)
    {
        for (int i = 0; i < MAX_MOTOR; i++)
        {
            pid_motor[i].target_val = target;
        }
    }
    // 如果电机ID不是最大电机数量，那么仅为目标电机设置目标值。
    else
    {
        pid_motor[motor_id].target_val = target;
    }
}
